/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-10-12 14:16:07
 * @LastEditors: LVGRAPE
 * @LastEditTime: 2025-06-04 10:47:28
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\position\position.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
/*
 * @Author: EltonLi lijiaming@uavi-tech.com
 * @Date: 2023-09-12 10:54:12
 * @LastEditors: EltonLi lijiaming@uavi-tech.com
 * @LastEditTime: 2023-12-18 17:06:39
 * @FilePath: \zino-fc-v4\applications\ZINO_FC_V4.0\position\position.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "position.h"
#include "fc_sensors.h"
#include "usercmd.h"
#include "maths.h"
#include "shanwai.h"
#include "fc_error.h"
#include "statecontrol.h"
#define DBG_TAG "position"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>
#include "rc.h"
#include "statecal.h"

extern float F1_wave[12];
// pid_obj_t pidData[PID_POSITION_VX];
// pid_obj_t pidData[PID_POSITION_VY];
// pid_obj_t pidData[PID_POSITION_VZ];

// pid_obj_t pidData[PID_POSITION_X];
// pid_obj_t pidData[PID_POSITION_Y];
// pid_obj_t pidData[PID_POSITION_Z];

float altHoldBaseThrottle = THRUST_BASE;

float altHoldBaseBiasRoll = 0;
float altHoldBaseBiasPitch = 0;

void positionPidInit(float dt)
{
    pidInit(&pidData[PID_POSITION_VX], &pidConfig()->pid[PID_POSITION_VX], dt);
    pidInit(&pidData[PID_POSITION_VY], &pidConfig()->pid[PID_POSITION_VY], dt);
    pidInit(&pidData[PID_POSITION_VZ], &pidConfig()->pid[PID_POSITION_VZ], dt);

    pidInit(&pidData[PID_POSITION_X], &pidConfig()->pid[PID_POSITION_X], dt);
    pidInit(&pidData[PID_POSITION_Y], &pidConfig()->pid[PID_POSITION_Y], dt);
    pidInit(&pidData[PID_POSITION_Z], &pidConfig()->pid[PID_POSITION_Z], dt);
}

void calculateAltHoldThrottle(float thrust, float velocity)
{
    static uint32_t thrustSum;
    static uint8_t thrustSumCnt;
    static float velocityPre;
    static uint16_t thrustPre;
    if (thrust > 500 && ABS(velocity) < 10)
    {
        thrustSum += thrust;
        thrustSumCnt++;
        if (thrustSumCnt > 100)
        {
            uint16_t thrustAverage = thrustSum / thrustSumCnt;
            thrustSum = 0;
            thrustSumCnt = 0;
            if (ABS(thrustAverage - altHoldBaseThrottle) < 50 && ABS(velocity - velocityPre) < 15.0f)
            {
                altHoldBaseThrottle = (float)(thrustAverage) * 0.2f + (float)thrustPre * 0.8f;
                altHoldBaseThrottle = constrainf(altHoldBaseThrottle, thrustPre - 20, thrustPre + 20);
                altHoldBaseThrottle = constrainf(altHoldBaseThrottle, 500, 950);
            }
        }
    }
    else
    {
        thrustSum = 0;
        thrustSumCnt = 0;
    }
    thrustPre = thrust;
    velocityPre = velocity;
}

static void velocityPidUpdate(setPoint_t *set, zdrone_state_t *state, float *thrust, float dt)
{
    static float thrustAdjLpf = 0;
    // static float velocityPre_y = 0;
    // static float vy_ki = 0;
    float rolladj = 0, pitchadj = 0, thrustadj = 0;
    if (pidConfig()->saveFlag == PID_CONFIG_SAVE_FLAG)
    {
        altHoldBaseThrottle = pidConfig()->baseThrottle;
    }
    if (set->manual[Y] != true)
    {
        // rolladj = -constrainf(pidUpdate(&pidData[PID_POSITION_VY], set->Vel.y - state->vel.y), -30.0f, 30.0f);
        rolladj = pid_calculate_kp(&pidData[PID_POSITION_VY], set->Vel.y, state->vel.y, dt, get_vy_kp_boost());
#if 1
        rolladj = -constrainf(rolladj, -30.0f, 30.0f);
        set->Angle.roll += (rolladj - set->Angle.roll) * 0.9f;
#else
        //加速度环，摇杆控制的是加速度
        rolladj = constrainf(rolladj, -5.0f, 5.0f);
        float err = rolladj - get_accy_lpf();
        vy_ki += (err * dt * 0.2f);
        set->Angle.roll = -constrainf((err * 0.5f + vy_ki), -30, 30);
        F1_wave[11] = vy_ki;
        if(!FC_ERROR_CHECK(FC_ARMED))
        {
            vy_ki = 0;
        }
#endif
    }
    else
    {
        // pidData[PID_POSITION_VY].inter = 0;
    }

    if (set->manual[X] != true)
    {
        // pitchadj = constrainf(pidUpdate(&pidData[PID_POSITION_VX], set->Vel.x - state->vel.x), -30.0f, 30.0f);
        pitchadj = pid_calculate_kp(&pidData[PID_POSITION_VX], set->Vel.x, state->vel.x, dt, get_vx_kp_boost());
        pitchadj = constrainf(pitchadj, -30.0f, 30.0f);
        set->Angle.pitch += (pitchadj - set->Angle.pitch) * 0.9f;
    }
    else
    {
        // pidData[PID_POSITION_VX].inter = 0;
    }

    // thrustadj = pidUpdate(&pidData[PID_POSITION_VZ], set->Vel.z - state->vel.z);
    thrustadj = pid_calculate(&pidData[PID_POSITION_VZ], set->Vel.z, state->vel.z, dt);
    thrustAdjLpf += (thrustadj - thrustAdjLpf) * 0.98f;
    *thrust = constrainf(thrustAdjLpf + altHoldBaseThrottle, altHoldBaseThrottle, 1000);

    // rt_printf_float("thrust ", *thrust, " \n");
#if 0
    static uint8_t dbCnt = 0;
    if (dbCnt++ >= 3)
    {
        dbCnt = 0;
        digitalOsc_Head();
        digitalOsc_float(state->vel.y);
        digitalOsc_float(pidData[PID_POSITION_VY].error);
        digitalOsc_float(pidData[PID_POSITION_VY].outP);
        digitalOsc_float(pidData[PID_POSITION_VY].inter);
        digitalOsc_float(pidData[PID_POSITION_VY].outI);
        digitalOsc_float(pidData[PID_POSITION_VY].outD);
        digitalOsc_float(pidData[PID_POSITION_VY].out);
        digitalOsc_End();
    }
#endif
}

void ManulLandingHandle(setPoint_t *set)
{
    static bool flag = false;
    if (set->Vel.z < -30.0f)
    {
        flag = true;
    }
    else
    {
        flag = false;
    }
    if (fc_sensor_data.tof.range <= 50 && flag)
    {
        flag = false;
        FC_ERROR_RESET(FC_ARMED);
        FC_ERROR_RESET(FC_OK_TO_ARM);
        FC_ERROR_RESET(FC_ARMING_ALERK);
    }
}

void positionPidUpdate(setPoint_t *set, zdrone_state_t *state, float *thrust, float dt)
{
    // if (usercmd.rcChannel[RC_KEY_VALUE] == RC_LK)
    // {
    //     AutoTakingOffHandle(setPoint,state);
    // }
    // if(usercmd.rcChannel[RC_KEY_VALUE] == RC_RK)
    // {
    //     AutoLandingHandle();
    // }
    /**
     * @brief
     *               ^X
     *               |
     *               |
     * Y<————————————|
     *
     */
    if (set->manual[X] != 1)
    {
        // set->Vel.x = pidUpdate(&pidData[PID_POSITION_X], set->Pos.x - state->pos.x);
        set->Vel.x = pid_calculate(&pidData[PID_POSITION_X], set->Pos.x, state->pos.x, dt);
    }
    else
    {
        // pidData[PID_POSITION_X].inter = 0;
    }
    if (set->manual[Y] != 1)
    {
        // set->Vel.y = pidUpdate(&pidData[PID_POSITION_Y], set->Pos.y - state->pos.y);
        set->Vel.y = pid_calculate(&pidData[PID_POSITION_Y], set->Pos.y, state->pos.y, dt);
    }
    else
    {
        // pidData[PID_POSITION_Y].inter = 0;
    }
    if (set->manual[Z] != 1)
    {
        // float adj = pidUpdate(&pidData[PID_POSITION_Z], set->Pos.z - state->pos.z);
        float adj = pid_calculate(&pidData[PID_POSITION_Z], set->Pos.z, state->pos.z, dt);
        set->Vel.z += (adj - set->Vel.z) * 0.98f;
        set->Vel.z = constrainf(set->Vel.z, -100.0f, 100.0f);
        // rt_printf_float("set->Pos.z ", set->Pos.z , " \n");
    }
    // ManulLandingHandle(set);
    velocityPidUpdate(set, state, thrust, dt);
}

void positionResetAllPID(void)
{
    pidReset(&pidData[PID_POSITION_VX]);
    pidReset(&pidData[PID_POSITION_VY]);
    pidReset(&pidData[PID_POSITION_VZ]);

    pidReset(&pidData[PID_POSITION_X]);
    pidReset(&pidData[PID_POSITION_Y]);
    pidReset(&pidData[PID_POSITION_Z]);
}
